#ifndef __JUMP_POINT_H__
#define __JUMP_POINT_H__

#include <ros/ros.h>
#include <iostream>
#include <sstream>
#include <algorithm>
#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>
#include <Eigen/Dense>

#include "../common/grid_point.h"
using namespace std;
using namespace cv;
using namespace Eigen; 

class JumpPointSearchPlanner{
    public:
        JumpPointSearchPlanner();
        ~JumpPointSearchPlanner();
        
        // 设置调试模式
        void SetDebugMode(bool mode);
        // 设置地图
        void SetMap(Mat map);
        // 计算到起点代价
        float CalCostG(cv::Point point, cv::Point start, float D);
        // 计算到终点代价
        float CalCostH(cv::Point point, cv::Point goal, float D);
        // 计算点是否有效
        bool IsValid(cv::Point point);
        // 判断两点是否相等
        bool IsEqual(cv::Point point1, cv::Point point2);
        // 规划路径
        vector<cv::Point> GetPath(cv::Point start, cv::Point goal, 
            vector<cv::Point>& expand_points, vector<cv::Point>& jump_points);
        // 计算跳点
        cv::Point JumpPointSearch(cv::Point curr_point, cv::Point dir_point, cv::Point goal, vector<cv::Point>& expand_points);
        // 检测强制邻居是否存在
        bool DetectForceNeighbor(cv::Point curr_point, cv::Point dir_point);

    private:
        bool __debug_mode;      // 调试模式
        long __debug_count;
        cv::Mat __map_mat;      // 地图数据
        vector<pair<cv::Point, float>> __neighbor_points;
        Eigen::MatrixXf __cost_cache_mat;
        
};

#endif 